import socket
import time
import threading
import robomaster
from robomaster import version
from robomaster import robot
import queue

line_num = 0

sdk_start = False
sdk_shutdown = False

msg_que = queue.Queue(50)
msg_lock = threading.Lock()

def parse_next_line(line_str):
    line = line_str.split(",")
    
    data_type = line[0]
    legal_data = True
    if data_type == "SENSOR_TYPE":
        for i in range(1,5):
            try:
                p = int(line[i])
            except:
                legal_data=False
    elif data_type == "ACTION_TYPE":
        if (len(line[1])!=1):
            legal_data = False
    elif data_type == "ANGLE_TYPE":
        try:
            p = float(line[1])
        except:
            legal_data=False

    if (legal_data):
        return line_str
    else:
        return "SKIP"

def send_msg():
    udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    
    ip = '127.0.0.1'
    port = 41234
    while True:
        global sdk_shutdown,sdk_start
        if (not sdk_start):continue
        if (sdk_shutdown):break

        msg_lock.acquire()
        if (msg_que.empty()):
            msg_lock.release()
            continue
        
        s = msg_que.get()
        msg_lock.release()
        msg = parse_next_line(s)

        if (msg=='SKIP'): continue
        udp_socket.sendto(("0 "+msg).encode("utf-8"), (ip, port))
        # time.sleep(0.02)
        if (msg=='EOF'):
            break

show_battery_info_once=False

def sub_battery_info_handler(batter_info, ep_robot):
    percent = batter_info
    brightness = int(percent * 255 / 100)
    # print("Battery: {0}%.".format(percent))'

    global show_battery_info_once
    if not show_battery_info_once:
        show_battery_info_once = True
        print("===========   Battery Print ================")
        print("")
        print("[localtime] {0}\n Battery = {1}%.".format(
            time.asctime(time.localtime()),
            percent))
        print("")
        print("===========Battery Print Success!===========")

    if (percent < 20): 
        ep_robot.led.set_led(comp="all", r=100, g=0, b=0)
        print("Battery: {0}%. 请充电！！请充电！！请充电！！".format(percent))
    else:
        ep_robot.led.set_led(comp="all", r=brightness, g=brightness, b=brightness)

def sub_sensor_info_handler(sub_info,log):
    F,R,B,L = sub_info
    s = "{},{},{},{},{}".format(
        # time.asctime(time.localtime()),
        "SENSOR_TYPE",
        F,B,L,R)

    print(s,file=log)
    global msg_lock,msg_que
    msg_lock.acquire()
    msg_que.put(s)
    msg_lock.release()


angle_init_once = False
angle_init_val=0
def sub_angle_info_handler(angle_info,log):
    a,b,c,d = angle_info
    global angle_init_once,angle_init_val
    if (not angle_init_once):
        angle_init_once = True
        angle_init_val = d
    
    s = "{},{:.3f}".format(
        # time.asctime(time.localtime()),
        "ANGLE_TYPE",
        d-angle_init_val)
    print(s, file=log)
    global msg_lock,msg_que
    msg_lock.acquire()
    msg_que.put(s)
    msg_lock.release()

xy_speed = 0.5
z_speed = 60
def do_action(log,action,ep_chassis,mute=False):
    move_dis = 0.2
    if (action == 'W'):
        if (not mute): print_action_state(log,action)
        ep_chassis.move(move_dis,0,0,xy_speed,z_speed).wait_for_completed()
        time.sleep(1)
    elif (action == 'S'):
        if (not mute): print_action_state(log,action)
        ep_chassis.move(-move_dis,0,0,xy_speed,z_speed).wait_for_completed()
        time.sleep(1)
    elif (action == 'A'):
        if (not mute): print_action_state(log,action)
        ep_chassis.move(0,-move_dis,0,xy_speed,z_speed).wait_for_completed()
        time.sleep(1)
    elif (action == 'D'):
        if (not mute): print_action_state(log,action)
        ep_chassis.move(0,move_dis,0,xy_speed,z_speed).wait_for_completed()
        time.sleep(1)
    elif (action == 'L'):
        if (not mute): print_action_state(log,action)
        ep_chassis.move(0,0,45,xy_speed,z_speed).wait_for_completed()
        time.sleep(1)
    elif (action == 'R'):
        if (not mute): print_action_state(log,action)
        ep_chassis.move(0,0,-45,xy_speed,z_speed).wait_for_completed()
        time.sleep(1)
    else:
        pass


def print_action_state(log,action):
    s="{},{}".format(
        # time.asctime(time.localtime()),
        "ACTION_TYPE",
        action)

    print(s,file=log)
    global msg_lock,msg_que
    msg_lock.acquire()
    msg_que.put(s)
    msg_lock.release()

def start_sdk():
    log = open("action.csv",'w')

    sdk_version = version.__version__
    print("sdk version:", sdk_version)

    ep_robot = robot.Robot()
    ep_robot.initialize(conn_type="sta")
    ep_robot.set_robot_mode(mode=robot.CHASSIS_LEAD)

    global sdk_start
    sdk_start = True
    
    print("===========connect success!===========")

    time.sleep(0.5)
    ep_version = ep_robot.get_version()
    print("- Robot Version: {0}".format(ep_version))
    time.sleep(0.5)

    # 低电量提示 - 电量<20% 在控制台提示充电
    ep_battery = ep_robot.battery
    ep_battery.sub_battery_info(5, sub_battery_info_handler, ep_robot)
    time.sleep(1)

    # # 测试修改后的 sensor API
    print("=========== Start Test ===========")
    ep_sensor = ep_robot.sensor
    ep_sensor.sub_distance(50, sub_sensor_info_handler,log)

    ep_gimbal = ep_robot.gimbal
    ep_gimbal.sub_angle(10,sub_angle_info_handler,log)

    
    ep_chassis = ep_robot.chassis
    # ep_chassis.sub_position(0,10,sub_pos_info_handler,log)

    while True:
        ss = input("action>")
        if (ss=='Q' or ss=='q'):
            break
        for ch in ss:
            do_action(log,ch,ep_chassis)
            # do_action(log,"L",ep_chassis,mute=True)
    
    print("=========== Chassis: move() end ===========")
    time.sleep(1)
    
    ep_sensor.unsub_distance()
    ep_gimbal.unsub_angle()
    # ep_chassis.unsub_position()

    global show_battery_info_once
    show_battery_info_once = False
    time.sleep(1)
    ep_battery.unsub_battery_info()
    ep_robot.close()

    global sdk_shutdown
    sdk_shutdown = True
 
def main():
    udp_t = threading.Thread(target=send_msg,args=())
    udp_t.start()

    sdk_t = threading.Thread(target=start_sdk,args=())
    sdk_t.start()
 
if __name__ == "__main__":
    main()